﻿#include "MobileFaceNet.h"

namespace onechchy
{
	MobileFaceNet::MobileFaceNet(const std::string& modelPath)
	{
		mNetResult = mMFaceNet.load_param((modelPath + "/mobilefacenet.param").c_str()) == 0;
		mNetResult = mMFaceNet.load_model((modelPath + "/mobilefacenet.bin").c_str()) == 0;
	}

	MobileFaceNet::~MobileFaceNet()
	{
		mMFaceNet.clear();
	}
	
	bool MobileFaceNet::loadResult()
	{
		return mNetResult;
	}

	/*
	欧几里得距离和余弦相似度各有优缺点，适用于不同的场景。以下是它们的详细对比：
欧几里得距离
优点：
1.	直观：计算两个点之间的直线距离，容易理解。
2.	适用性广：适用于各种需要计算实际距离的场景。
缺点：
1.	对尺度敏感：不同尺度的数据会影响结果，需要进行归一化处理。
2.	高维空间效果差：在高维空间中，欧几里得距离可能会失去区分度。
适用场景：
•	几何空间：适用于需要计算实际物理距离的场景，如地图上的两点距离。
•	聚类分析：在K-means等聚类算法中常用。
余弦相似度
优点：
1.	对尺度不敏感：只关注向量的方向，不受向量长度影响。
2.	高维空间效果好：在高维空间中仍能有效区分向量。
缺点：
1.	不适用于零向量：零向量的余弦相似度未定义。
2.	不适用于需要计算实际距离的场景：只适用于衡量方向相似度。
适用场景：
•	文本相似度：在自然语言处理（NLP）中，用于计算文档或句子的相似度。
•	推荐系统：用于计算用户与物品的相似度，推荐相似的物品。
总结
•	欧几里得距离：适用于需要计算实际距离的场景，适合低维数据。
•	余弦相似度：适用于衡量方向相似度的场景，适合高维数据和文本相似度计算。
选择哪种方法取决于具体的应用需求和数据特性。

	*/
	double MobileFaceNet::calculSimilar(std::vector<float>& v1, std::vector<float>& v2, int distance)
	{
		if (v1.size() != v2.size() || !v1.size())
		{
			return 0;
		}

		double ret = 0.0, mod1 = 0.0, mod2 = 0.0, dist = 0.0, diff = 0.0;

		// disttance 替换为枚举
		if (distance == 0)
		{
			// 计算欧几里得距离
			for (std::vector<double>::size_type i = 0; i != v1.size(); ++i)
			{
				diff = v1[i] - v2[i];
				dist += (diff * diff);
			}

			dist = sqrt(dist);
		}
		else
		{
			// 计算余弦相似度
			for (std::vector<double>::size_type i = 0; i != v1.size(); ++i)
			{
				ret += v1[i] * v2[i];
				mod1 += v1[i] * v1[i];
				mod2 += v2[i] * v2[i];
			}

			dist = ret / (sqrt(mod1) * sqrt(mod2));
		}

		return dist;
	}

	float calculateMean(const ncnn::Mat& mat)
	{
		// 获取维度信息
		int w = mat.w;
		int h = mat.h;
		int c = mat.c;

		float sum = 0.0f;
		int count = 0;

		// 遍历每个通道
		for (int q = 0; q < c; q++)
		{
			const float* ptr = mat.channel(q);

			// 遍历每个像素
			for (int y = 0; y < h; y++)
			{
				for (int x = 0; x < w; x++)
				{
					sum += ptr[y * w + x];
					count++;
				}
			}
		}

		// 计算平均值
		return count > 0 ? sum / count : 0.0f;
	}



	void MobileFaceNet::start(ncnn::Mat& ncnnImg, std::vector<float>& feature128)
	{
		feature128.resize(128);
		ncnn::Extractor ex = mMFaceNet.create_extractor();
		ex.set_num_threads(mThreadNum);
		ex.set_light_mode(true);
		ex.input("data", ncnnImg);
		ncnn::Mat out;
		ex.extract("fc1", out);
		for (int j = 0; j < 128; j++)
		{
			feature128[j] = out[j];
		}
		normalize(feature128);
	}

	void MobileFaceNet::normalize(std::vector<float>& feature)
	{
		float sum = 0;
		for (auto it = feature.begin(); it != feature.end(); it++)
		{
			sum += (float)*it * (float)*it;
		}
		sum = sqrt(sum);
		for (auto it = feature.begin(); it != feature.end(); it++)
		{
			*it /= sum;
		}
	}

	void MobileFaceNet::setThreadNum(int threadNum)
	{
		this->mThreadNum = threadNum;
	}

	ncnn::Mat MobileFaceNet::preprocess(ncnn::Mat mat, std::vector<int> info)
	{
		const int targetWidth = 112;
		const int targetHeight = 112;

		float dst[10] = {
			30.2946, 65.5318, 48.0252, 33.5493, 62.7299,
			51.6963, 51.5014, 71.7366, 92.3655, 92.2041
		};

		if (targetWidth == 112)
		{
			for (int i = 0; i < 5; i++)
			{
				dst[i] += 8.0;
			}
		}

		float src[10];
		for (int i = 0; i < 10; i++)
		{
			src[i] = info[i];
		}

		float M[6];
		getAffineMatrix(src, dst, M);
		ncnn::Mat out;
		warpAffineMatrix(mat, out, M, targetWidth, targetHeight);
		return out;
	}

	void MobileFaceNet::warpAffineMatrix(ncnn::Mat src, ncnn::Mat& dst, float* M, int dstW, int dstH)
	{
		int srcW = src.w;
		int srcH = src.h;

		unsigned char* srcU = new unsigned char[srcW * srcH * 3] {0};
		unsigned char* dstU = new unsigned char[dstW * dstH * 3] {0};

		src.to_pixels(srcU, ncnn::Mat::PIXEL_RGB);

		float m[6];
		for (int i = 0; i < 6; i++)
		{
			m[i] = M[i];
		}

		float D = m[0] * m[4] - m[1] * m[3];
		D = D != 0 ? 1. / D : 0;
		float A11 = m[4] * D, A22 = m[0] * D;
		m[0] = A11, m[1] *= -D, m[3] *= -D, m[4] = A22;
		float b1 = -m[0] * m[2] - m[1] * m[5];
		float b2 = -m[3] * m[2] - m[4] * m[5];
		m[2] = b1; 
		m[5] = b2;

		for (int y = 0; y < dstH; y++)
		{
			for (int x = 0; x < dstW; x++)
			{
				float fx = m[0] * x + m[1] * y + m[2];
				float fy = m[3] * x + m[4] * y + m[5];

				int sy = (int)floor(fy);
				fy -= sy;
				if (sy < 0 || sy >= srcH)
				{
					continue;
				}

				short cbufy[2];
				cbufy[0] = (short)((1.f - fy) * 2048);
				cbufy[1] = 2048 - cbufy[0];

				int sx = (int)floor(fx);
				fx -= sx;
				if (sx < 0 || sx >= srcW)
				{
					continue;
				}

				short cbufx[2];
				cbufx[0] = (short)((1.f - fx) * 2048);
				cbufx[1] = 2048 - cbufx[0];

				if (sy == srcH - 1 || sx == srcW - 1)
				{
					continue;
				}

				for (int c = 0; c < 3; c++)
				{
					dstU[3 * (y * dstW + x) + c] = (
						srcU[3 * (sy * srcW + sx) + c] * cbufx[0] * cbufy[0] +
						srcU[3 * ((sy + 1) * srcW + sx) + c] * cbufx[0] * cbufy[1] +
						srcU[3 * (sy * srcW + sx + 1) + c] * cbufx[1] * cbufy[0] +
						srcU[3 * ((sy + 1) * srcW + sx + 1) + c] * cbufx[1] * cbufy[1]
						) >> 22;
				}
			}
		}

		dst = ncnn::Mat::from_pixels(dstU, ncnn::Mat::PIXEL_RGB, dstW, dstH);
		//float meanNcnnImg = calculateMean(dst);
		delete[] srcU;
		delete[] dstU;
	}

	void MobileFaceNet::getAffineMatrix(float* src_5pts, const float* dst_5pts, float* M)
	{
		float src[10], dst[10];
		memcpy(src, src_5pts, sizeof(float) * 10);
		memcpy(dst, dst_5pts, sizeof(float) * 10);

		float ptmp[2] = { 0 };
		for (int i = 0; i < 5; i++)
		{
			ptmp[0] += src[i];
			ptmp[1] += src[i + 5];
		}

		ptmp[0] /= 5;
		ptmp[1] /= 5;
		for (int i = 0; i < 5; ++i)
		{
			src[i] -= ptmp[0];
			src[i + 5] -= ptmp[1];
			dst[i] -= ptmp[0];
			dst[i + 5] -= ptmp[1];
		}

		float dstX = (dst[3] + dst[4] - dst[0] - dst[1]) / 2, dstY = (dst[8] + dst[9] - dst[5] - dst[6]) / 2;
		float srcX = (src[3] + src[4] - src[0] - src[1]) / 2, srcY = (src[8] + src[9] - src[5] - src[6]) / 2;
		float theta = atan2(dstX, dstY) - atan2(srcX, srcY);

		float scale = sqrt(pow(dstX, 2) + pow(dstY, 2)) / sqrt(pow(srcX, 2) + pow(srcY, 2));
		float pts1[10];
		float pts0[2];

		float _a = sin(theta), _b = cos(theta);
		pts0[0] = pts0[1] = 0;
		for (int i = 0; i < 5; ++i)
		{
			pts1[i] = scale * (src[i] * _b + src[i + 5] * _a);
			pts1[i + 5] = scale * (-src[i] * _a + src[i + 5] * _b);
			pts0[0] += (dst[i] - pts1[i]);
			pts0[1] += (dst[i + 5] - pts1[i + 5]);
		}

		pts0[0] /= 5;
		pts0[1] /= 5;

		float sqloss = 0;
		for (int i = 0; i < 5; ++i)
		{
			sqloss += ((pts0[0] + pts1[i] - dst[i]) * (pts0[0] + pts1[i] - dst[i])
				+ (pts0[1] + pts1[i + 5] - dst[i + 5]) * (pts0[1] + pts1[i + 5] - dst[i + 5]));
		}

		float square_sum = 0;
		for (int i = 0; i < 10; ++i)
		{
			square_sum += src[i] * src[i];
		}

		for (int t = 0; t < 200; ++t)
		{
			_a = 0;
			_b = 0;
			for (int i = 0; i < 5; ++i)
			{
				_a += ((pts0[0] - dst[i]) * src[i + 5] - (pts0[1] - dst[i + 5]) * src[i]);
				_b += ((pts0[0] - dst[i]) * src[i] + (pts0[1] - dst[i + 5]) * src[i + 5]);
			}

			if (_b < 0)
			{
				_b = -_b;
				_a = -_a;
			}

			float _s = sqrt(_a * _a + _b * _b);
			_b /= _s;
			_a /= _s;

			for (int i = 0; i < 5; ++i)
			{
				pts1[i] = scale * (src[i] * _b + src[i + 5] * _a);
				pts1[i + 5] = scale * (-src[i] * _a + src[i + 5] * _b);
			}

			float _scale = 0;
			for (int i = 0; i < 5; ++i)
			{
				_scale += ((dst[i] - pts0[0]) * pts1[i] + (dst[i + 5] - pts0[1]) * pts1[i + 5]);
			}

			_scale /= (square_sum * scale);
			for (int i = 0; i < 10; ++i)
			{
				pts1[i] *= (_scale / scale);
			}
			scale = _scale;

			pts0[0] = pts0[1] = 0;
			for (int i = 0; i < 5; ++i)
			{
				pts0[0] += (dst[i] - pts1[i]);
				pts0[1] += (dst[i + 5] - pts1[i + 5]);
			}
			pts0[0] /= 5;
			pts0[1] /= 5;

			float _sqloss = 0;
			for (int i = 0; i < 5; ++i)
			{
				_sqloss += ((pts0[0] + pts1[i] - dst[i]) * (pts0[0] + pts1[i] - dst[i])
					+ (pts0[1] + pts1[i + 5] - dst[i + 5]) * (pts0[1] + pts1[i + 5] - dst[i + 5]));
			}
			if (abs(_sqloss - sqloss) < 1e-2)
			{
				break;
			}
			sqloss = _sqloss;
		}

		for (int i = 0; i < 5; ++i)
		{
			pts1[i] += (pts0[0] + ptmp[0]);
			pts1[i + 5] += (pts0[1] + ptmp[1]);
		}

		M[0] = _b * scale;
		M[1] = _a * scale;
		M[3] = -_a * scale;
		M[4] = _b * scale;
		M[2] = pts0[0] + ptmp[0] - scale * (ptmp[0] * _b + ptmp[1] * _a);
		M[5] = pts0[1] + ptmp[1] - scale * (-ptmp[0] * _a + ptmp[1] * _b);

	}

}
